/**
 * anis.darwich
 */

package clubhouse.visualisation;

import java.awt.BasicStroke;
import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.Label;
import java.awt.TextArea;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.awt.event.MouseEvent;
import java.awt.event.MouseListener;
import java.awt.geom.GeneralPath;
import java.awt.geom.Line2D;
import java.awt.geom.Point2D;
import java.awt.geom.Rectangle2D;
import java.io.File;
import java.util.Vector;

import javax.swing.BoxLayout;
import javax.swing.JApplet;
import javax.swing.JButton;
import javax.swing.JCheckBox;
import javax.swing.JComboBox;
import javax.swing.JFileChooser;
import javax.swing.JFrame;
import javax.swing.JLabel;
import javax.swing.JPanel;
import javax.swing.JScrollPane;
import javax.swing.JTextField;
import javax.swing.JTextPane;
import javax.swing.SwingUtilities;
import javax.swing.text.DefaultStyledDocument;
import javax.swing.text.Style;
import javax.swing.text.StyleConstants;
import javax.swing.text.StyleContext;

import clubhouse.base.Base;
import clubhouse.control.DataChecker;

public class WSNexperimentation extends JApplet implements Runnable, ActionListener, MouseListener {
	
	private static final long serialVersionUID = 1L;
	final JFileChooser fc = new JFileChooser();
	
	private JCheckBox viewSimulation = new JCheckBox("Simulate");
	private JCheckBox viewLandmarks = new JCheckBox("Landmarks");
	private JCheckBox viewOdometry = new JCheckBox("Robot position");
	private JCheckBox viewTrajectory = new JCheckBox("Trajectory");
	private JCheckBox viewReal = new JCheckBox("Initial Trajectory");
	private JCheckBox viewError = new JCheckBox("Error Trajectory");
	private JCheckBox viewFilter = new JCheckBox("EKF Trajectory");
	private JCheckBox viewAnti = new JCheckBox("A-S Trajectory");
	private JCheckBox viewVoronoi = new JCheckBox("Show Voronoi");
	private JCheckBox viewRange = new JCheckBox("Show Landmarks' Range");
	private JCheckBox enableAntiSlam = new JCheckBox("Anti-Slam Process");
	private JCheckBox enableError = new JCheckBox("Transmission Error");
	private JCheckBox enableEkf = new JCheckBox("EKF");
	
	private JComboBox scenarioChoice = new JComboBox(new String[]{"1","2","3"});
	private JTextField errorFactor = new JTextField("10", 3);
	private JComboBox landmarkChoice = new JComboBox(new String[]{"Random","From file","From panel"});
	private JTextField numberOfLandmarks = new JTextField("10", 3);
	private JButton openFile = new JButton("Open file");
	private JButton saveFile = new JButton("Save file");
	
	
	private JButton startButton = new JButton("Start");
	private JButton stopButton = new JButton("Stop");
	private JButton resumeButton = new JButton("Resume");
	private JButton clearButton = new JButton("Clear");
	
	private TextArea messageLog = new TextArea();
	private JTextPane debugLog = new JTextPane();
	private TextArea M1Display = new TextArea();
	private TextArea M2Display = new TextArea();
	private TextArea M3Display = new TextArea();
	private TextArea M4Display = new TextArea();
	private JScrollPane scrollDebugPane;
	
	public Base test = new Base(2, this);
	public Thread t;
	private DataChecker checker;
	private int scenario=3;
	
	private Vector<double[]> userLandmarks = new Vector<double[]>();
	private String userFileName = "";
	private int userId = 0;
	
	private DrawPanel experimentation = new DrawPanel(this,this.scenario);
	
	public void init () {
        try {
        	SwingUtilities.invokeAndWait(this);
        } catch (Exception e) {
        	System.err.println("Initialization failure");
        }
    }
	
	public void run() {
		this.t = new Thread(test);
		
		/*for JtextPanel*/
		Style defaultStyle = this.debugLog.getStyle(StyleContext.DEFAULT_STYLE);
        Style style = this.debugLog.addStyle("paragraph", defaultStyle);

        Object o=this.debugLog.getParagraphAttributes().getAttribute(StyleConstants.NameAttribute);
        style.addAttribute(StyleConstants.NameAttribute, o); //replace the name with proper tag
		this.checker = new DataChecker(style,((!this.isAntiSlamed())&&(this.scenario==1)));
		/**/
		
		setLayout(new BorderLayout());
		
		JPanel scenarioPanel = new JPanel();
        scenarioPanel.add(new JLabel("Scenario "), "West");
        scenarioPanel.add(scenarioChoice, "East");
        
        JPanel landmarksPanel = new JPanel();
        landmarksPanel.add(new JLabel("Landmarks "));
        landmarksPanel.add(landmarkChoice);
        landmarksPanel.add(new JLabel("Number of Landmarks "));
        landmarksPanel.add(numberOfLandmarks);
        numberOfLandmarks.setEditable(false);
        
        JPanel filePanel = new JPanel();
        filePanel.add(openFile, "West");
        filePanel.add(saveFile, "East");
        openFile.setEnabled(false);
        saveFile.setEnabled(false);
        
        JPanel errorPanel = new JPanel();
        errorPanel.add(new JLabel("Error attenuation factor "), "West");
        errorPanel.add(errorFactor, "East");
		
		JPanel paramatersPanel = new JPanel();
		paramatersPanel.setLayout(new BoxLayout(paramatersPanel, BoxLayout.X_AXIS));
		paramatersPanel.add(scenarioPanel);
		//paramatersPanel.add(landmarksPanel);
		//paramatersPanel.add(errorPanel);
		//paramatersPanel.add(filePanel);
		
		
		JPanel buttonPanel = new JPanel();
		buttonPanel.add(startButton);
        buttonPanel.add(stopButton);
        buttonPanel.add(resumeButton);
        buttonPanel.add(new JLabel("          "));
        buttonPanel.add(clearButton);
        buttonPanel.add(new JLabel("          "));
        buttonPanel.add(paramatersPanel);
        stopButton.setEnabled(false);
        resumeButton.setEnabled(false);
        clearButton.setEnabled(false);
        this.add(buttonPanel, "North");
        
        JPanel switchPanel = new JPanel();
        switchPanel.setLayout(new BoxLayout(switchPanel, BoxLayout.Y_AXIS));
        switchPanel.add(viewSimulation);
        switchPanel.add(new JLabel("  "));
        switchPanel.add(viewLandmarks);
        switchPanel.add(viewOdometry);
        switchPanel.add(viewTrajectory);
        switchPanel.add(viewRange);
        switchPanel.add(viewVoronoi);
        switchPanel.add(viewReal);
        switchPanel.add(viewError);
        switchPanel.add(viewFilter);
        switchPanel.add(viewAnti);
        switchPanel.add(new JLabel("  "));
        switchPanel.add(enableError);
        switchPanel.add(enableEkf);
        switchPanel.add(enableAntiSlam);
        viewVoronoi.setVisible(false);
        viewRange.setVisible(false);
		viewReal.setVisible(false);
		viewError.setVisible(false);
		viewFilter.setVisible(false);
		viewAnti.setVisible(false);
        viewLandmarks.setSelected(true);
        viewOdometry.setSelected(true);
        viewSimulation.setSelected(false);
        viewTrajectory.setSelected(true);
        enableEkf.setSelected(false);
        this.add(switchPanel, "West");

        JPanel macroPanel = new JPanel();
        macroPanel.setLayout(new BorderLayout());
        
        //Debug Log Display
        JPanel tempPanel1 = new JPanel();
        tempPanel1.setLayout(new BorderLayout());
        JPanel debugPanel = new JPanel();
        debugPanel.setLayout(new BorderLayout());
        debugPanel.add(new Label("Debug Log:"), "North");
        debugLog.setBackground(Color.white);
        debugLog.setEditable(false);
        debugLog.setPreferredSize(new Dimension(600,250));
        debugLog.setContentType("text/html");
                
        scrollDebugPane = new JScrollPane(debugLog);
        debugPanel.add(scrollDebugPane);
        //debugPanel.add(scrollDebugPane, "West");
        //debugPanel.add(new JLabel("   "), "Center");
        tempPanel1.add(debugPanel, "West");
        tempPanel1.add(new JLabel("   "), "Center");
        //macroPanel.add(debugPanel,"West");
        
        //M1 Display       
        //JPanel tempPanel1 = new JPanel();
        JPanel M1Panel = new JPanel();
        M1Panel.setLayout(new BorderLayout());
        M1Panel.add(new Label("M1:"), "North");
        M1Panel.setPreferredSize(new Dimension(150,250));
        this.M1Display.setBackground(Color.white);
        this.M1Display.setEditable(false);
        M1Panel.add(M1Display);
        tempPanel1.add(M1Panel, "East"); 
        //macroPanel.add(M1Panel,"Center");   
        
        //M2 Display       
        JPanel tempPanel2 = new JPanel();
        tempPanel2.setLayout(new BorderLayout());
        JPanel M2Panel = new JPanel();
        M2Panel.setLayout(new BorderLayout());
        M2Panel.add(new Label("M2:"), "North");
        this.M2Display.setBackground(Color.white);
        this.M2Display.setEditable(false);
        this.M2Display.setPreferredSize(new Dimension(150,250));
        M2Panel.add(M2Display);
        tempPanel2.add(new JLabel("   "), "West"); 
        tempPanel2.add(M2Panel, "Center");  
        tempPanel2.add(new JLabel("   "), "East"); 
        //macroPanel.add(M2Panel,"East");  
        
        //M3 Display       
        JPanel tempPanel3 = new JPanel();
        tempPanel3.setLayout(new BorderLayout());
        JPanel M3Panel = new JPanel();
        M3Panel.setLayout(new BorderLayout());
        M3Panel.add(new Label("M3:"), "North");
        this.M3Display.setBackground(Color.white);
        this.M3Display.setEditable(false);
        this.M3Display.setPreferredSize(new Dimension(150,250));
        M3Panel.add(M3Display, "Center");
        tempPanel3.add(M3Panel,"West");  
        tempPanel3.add(new JLabel("   "), "Center"); 
        
        //M4 Display       
        JPanel M4Panel = new JPanel();
        M4Panel.setLayout(new BorderLayout());
        M4Panel.add(new Label("M4:"), "North");
        this.M4Display.setBackground(Color.white);
        this.M4Display.setEditable(false);
        this.M4Display.setPreferredSize(new Dimension(150,250));
        M4Panel.add(M4Display, "Center");
        tempPanel3.add(M4Panel,"East");  
        
        macroPanel.add(tempPanel1, "West");
        macroPanel.add(tempPanel2, "Center");
        macroPanel.add(tempPanel3, "East");
        this.add(macroPanel, "South");
        
        JPanel messagePanel = new JPanel();
        messagePanel.setLayout(new BorderLayout());
        messagePanel.add(new Label("Message Log:"), "North");
        messageLog.setEnabled(false);
        messageLog.setBackground(Color.white);
        messagePanel.add(messageLog, "Center");
        this.add(messagePanel, "East");
        
        experimentation.setBackground(Color.lightGray);
        this.add(experimentation, "Center");
        
        startButton.addActionListener(this);
        stopButton.addActionListener(this);
        clearButton.addActionListener(this);
        resumeButton.addActionListener(this);
        viewLandmarks.addActionListener(this);
        viewOdometry.addActionListener(this);
        viewSimulation.addActionListener(this);
        viewTrajectory.addActionListener(this);
        enableAntiSlam.addActionListener(this);
        enableError.addActionListener(this);
        enableEkf.addActionListener(this);
        viewAnti.addActionListener(this);
        viewReal.addActionListener(this);
		viewError.addActionListener(this);
		viewFilter.addActionListener(this);
        scenarioChoice.addActionListener(this);
        viewVoronoi.addActionListener(this);
        viewRange.addActionListener(this);
        landmarkChoice.addActionListener(this);
        openFile.addActionListener(this);
        saveFile.addActionListener(this);
        
        experimentation.addMouseListener(this);
	}

	public void actionPerformed(ActionEvent e) {
		if (e.getSource() == clearButton) {
			//Clear stuff
			this.userId = 0;
			experimentation.clear();
			test.clearRoutine();
	        this.messageLog.setText("");
	        this.debugLog.setText("");
	        this.checker.clear();
			
			//Run new stuff
			this.test = new Base(2, this);
			this.t = new Thread(test);	        
	        
			//Manage buttons
			stopButton.setEnabled(false);
	        resumeButton.setEnabled(false);
	        clearButton.setEnabled(false);
	        startButton.setEnabled(true);
			scenarioChoice.setEnabled(true);
			errorFactor.setEnabled(true);
		} else if (e.getSource() == startButton) {
			//Manage buttons
			stopButton.setEnabled(true);
			startButton.setEnabled(false);
			resumeButton.setEnabled(false);
	        clearButton.setEnabled(false);
			scenarioChoice.setEnabled(false);
			landmarkChoice.setEnabled(false);
			errorFactor.setEnabled(false);
			
			//Init infos
			//this.userId = 0;
			test.setAntiSlam(this.isAntiSlamed());
			this.scenario = this.scenarioChoice.getSelectedIndex()+1;
			this.checker.setDoCheck(((!this.isAntiSlamed())&&(this.scenario==1)), this.scenario);	//if Anti-Slam enable or scenario 2, don't check data
			test.setError(this.isErrorEnabled());
			test.setErrorFactor(Integer.parseInt(errorFactor.getText()));
			this.checker.setErrorFactor(Integer.parseInt(errorFactor.getText()));
			this.experimentation.setDecalage(this.scenario);
			this.test.setScenario(this.scenario);
			this.test.setCompleteSimulation(isCompleteSimulation());
			
			/*
			switch (this.landmarkChoice.getSelectedIndex()) {
			case 0:
				//call function with appropriate set of variables
				this.test.manageLandmarks(0, "", Integer.parseInt(numberOfLandmarks.getText()), null);
				break;
			case 1:
				//call function with appropriate set of variables
				break;
			case 2:
				//call function with appropriate set of variables
				this.test.manageLandmarks(2, "", 0, this.userLandmarks);
				break;
			default:
				break;
			}*/
			
			//Run experimentation
			this.t.start();
			experimentation.repaint();
		} else if (e.getSource() == stopButton) {
			test.stopRoutine();
	        resumeButton.setEnabled(true);
	        clearButton.setEnabled(true);
	        stopButton.setEnabled(false);
	        startButton.setEnabled(false);
	        this.addStopInfo();
	        //this.addDebugLog2(DataChecker.stopInfo, null);
			experimentation.repaint();
		} else if (e.getSource() == resumeButton) {
			test.resumeRoutine();
			resumeButton.setEnabled(false);
	        clearButton.setEnabled(false);
	        startButton.setEnabled(false);
	        stopButton.setEnabled(true);
	        experimentation.repaint();
		} else if (e.getSource() == viewLandmarks) {
			experimentation.repaint();
		} else if (e.getSource() == viewOdometry) {
			experimentation.repaint();
		} else if (e.getSource() == viewSimulation) {
			if (isCompleteSimulation()) {
				this.scenarioChoice.setSelectedIndex(2);
				
				viewAnti.setVisible(true);
		        viewReal.setVisible(true);
				viewError.setVisible(true);
				viewFilter.setVisible(true);
				enableAntiSlam.setVisible(false);
				enableError.setVisible(false);
				enableEkf.setVisible(false);
				viewTrajectory.setVisible(false);
			} else {
				viewAnti.setVisible(false);
		        viewReal.setVisible(false);
				viewError.setVisible(false);
				viewFilter.setVisible(false);
				enableAntiSlam.setVisible(true);
				enableError.setVisible(true);
				enableEkf.setVisible(true);
				viewTrajectory.setVisible(true);
			}
		} else if (e.getSource() == viewTrajectory) {
			experimentation.repaint();
		} else if (e.getSource() == enableAntiSlam) {
			test.setAntiSlam(this.isAntiSlamed());
			this.checker.setDoCheck(((!this.isAntiSlamed())&&(this.scenario!=2)),this.scenario);	//if Anti-Slam enable, don't check data
		} else if (e.getSource() == enableError) {
			test.setError(this.isErrorEnabled());
		} else if (e.getSource() == enableEkf) {
			test.setEkf(this.isEkfEnabled());
		} else if (e.getSource() == this.scenarioChoice) {
			this.scenario = this.scenarioChoice.getSelectedIndex()+1;
			if (this.scenario==1){
				viewVoronoi.setVisible(false);
				viewRange.setVisible(false);
				viewRange.setSelected(false);
			} else {
				viewVoronoi.setVisible(true);
				viewRange.setVisible(true);
				viewRange.setSelected(true);
			}
		} else if (e.getSource() == this.viewVoronoi) {
			experimentation.repaint();
		} else if (e.getSource() == this.viewAnti) {
			experimentation.repaint();
		} else if (e.getSource() == this.viewReal) {
			experimentation.repaint();
		} else if (e.getSource() == this.viewError) {
			experimentation.repaint();
		} else if (e.getSource() == this.viewFilter) {
			experimentation.repaint();
		} else if (e.getSource() == this.viewRange) {
			experimentation.repaint();
		} else if (e.getSource() == landmarkChoice) {
			/*
			switch (this.landmarkChoice.getSelectedIndex()) {
			case 0:
				saveFile.setEnabled(true);
				openFile.setEnabled(false);
				numberOfLandmarks.setEditable(true);
				break;
			case 1:
				openFile.setEnabled(true);
				saveFile.setEnabled(false);
				numberOfLandmarks.setEditable(false);
				break;
			case 2:
				saveFile.setEnabled(true);
				openFile.setEnabled(false);
				numberOfLandmarks.setEditable(false);
				break;
			default:
				break;
			}*/
			
		} else if (e.getSource() == openFile) {
			/*
			int returnVal = fc.showOpenDialog(WSNexperimentation.this);
			if (returnVal == JFileChooser.APPROVE_OPTION) {
	            File file = fc.getSelectedFile();
	            this.userFileName = file.getName();
	        } else {
	        	this.userFileName = "";
	        }*/
		} else if (e.getSource() == saveFile) {
			
		}
	}

	public void mouseClicked(MouseEvent arg0) {}
	public void mouseEntered(MouseEvent arg0) {}
	public void mouseExited(MouseEvent arg0) {}
	public void mouseReleased(MouseEvent arg0) {}
	public void mousePressed(MouseEvent arg0) {
		/*
		if (arg0.getSource() != experimentation) return;
		if(this.landmarkChoice.getSelectedIndex() == 2) {
			int[] f = experimentation.getDecalage();
			this.userLandmarks.add(new double[] {this.userId, (arg0.getX()-f[1])/f[0], -(arg0.getY()-f[2])/f[0]});
			this.userId++;
		}
		this.updateDrawing(null, this.userLandmarks, null, null, null);
		*/
	}
	
	public boolean isLandmarks() {
		return viewLandmarks.isSelected();
	}
	public boolean isOdometry() {
		return viewOdometry.isSelected();
	}
	public boolean isSimulate() {
		return viewSimulation.isSelected();
	}
	public boolean isTrajectory() {
		return viewTrajectory.isSelected();
	}
	public boolean isAntiSlamed() {
		return enableAntiSlam.isSelected();
	}
	public boolean isErrorEnabled() {
		return enableError.isSelected();
	}
	public boolean isEkfEnabled() {
		return enableEkf.isSelected();
	}
	public boolean isCompleteSimulation() {
		return viewSimulation.isSelected();
	}
	public boolean viewVoronoiEnabled() {
		return this.viewVoronoi.isSelected();
	}
	
	public boolean viewRangeEnabled() {
		return this.viewRange.isSelected();
	}
	
	public boolean viewAntiEnabled() {
		return this.viewAnti.isSelected();
	}
	
	public boolean viewRealEnabled() {
		return this.viewReal.isSelected();
	}
    
	public boolean viewErrorEnabled() {
		return this.viewError.isSelected();
	}
	
	public boolean viewFilterEnabled() {
		return this.viewFilter.isSelected();
	}
	
	public void addMsgLog(String text) {
		this.messageLog.append(text);
	}
	
	public void addStopInfo(){
		this.M1Display.append(this.checker.verifyData(DataChecker.displayM1));
		this.M2Display.append(this.checker.verifyData(DataChecker.displayM2));
		this.M3Display.append(this.checker.verifyData(DataChecker.displayM3));
		this.M4Display.append(this.checker.verifyData(DataChecker.displayM4));
		this.checker.saveInfo();
	}
	
	
	public void addDebugLog2(int type, double... data) {
		String text = null;
		text = this.checker.verifyData(type, data);
		try {
			DefaultStyledDocument doc = (DefaultStyledDocument) this.debugLog.getDocument ();
			int initPos = doc.getEndPosition().getOffset();
		
			doc.insertString(initPos, text, this.checker.getCurrentStyle());
			this.debugLog.setCaretPosition(doc.getEndPosition().getOffset()-1);	//autoscroll
			
		} catch (Exception e) {
			e.printStackTrace();
		}
	}
	
	public void updateDrawing(Vector<double[]> position, Vector<double[]> landmarks, Vector<double[]> errorPosition, Vector<double[]> estimatePosition, Vector<Point2D[]> polygons) {
		experimentation.updateData(position, landmarks, errorPosition, estimatePosition, polygons);
		experimentation.repaint();
	}
	
	public static void main (String[] args) {
		WSNexperimentation applet = new WSNexperimentation();
        applet.init();
        JFrame dWindow = new JFrame();
        dWindow.setExtendedState(dWindow.getExtendedState()|JFrame.MAXIMIZED_BOTH);
        dWindow.setTitle("WSN Experimentation");
        dWindow.setLayout(new BorderLayout());
        dWindow.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
        dWindow.add(applet, "Center");
        dWindow.add(new Label(" Lionel Croix | Anis Darwich | Simulation and experimentation on Robot in WSN | WnSN Lab | SJTU "), "South");
        dWindow.setVisible(true);
    }
}


class DrawPanel extends JPanel {

	private static final long serialVersionUID = 1L;
	
	private WSNexperimentation xPeriment;
	
	private int scenario;
	private int facteur;
	private int xDECALAGE;
	private int yDECALAGE;
	
	private Graphics2D g;
	
	public Vector<double[]> position = null;
	public Vector<double[]> landmarks = null;
	public Vector<double[]> estimate = null;
	public Vector<double[]> error = null;
	public Vector<Point2D[]> polygons;
	
	public GeneralPath path = new GeneralPath(GeneralPath.WIND_EVEN_ODD);

	
	public DrawPanel(WSNexperimentation wsNexperimentation,int scenario) {
		this.xPeriment = wsNexperimentation;
		this.setDecalage(scenario);
	}
	
	public void clear() {
		this.position = null;
		this.landmarks = null;
		this.estimate = null;
		this.error = null;
		this.polygons = null;
		this.repaint();
	}

	public void updateData(Vector<double[]> position, Vector<double[]> landmarks, Vector<double[]> errorPosition, Vector<double[]> estimatePosition, Vector<Point2D[]> polygons) {
		this.position = position;
		this.landmarks = landmarks;
		this.estimate = estimatePosition;
		this.error = errorPosition;
		this.polygons = polygons;
	}
	
	public void setDecalage(int scenario){
		this.scenario = scenario;
		if (this.scenario==1){
			this.facteur = 10;
			this.xDECALAGE = 20*this.facteur;
			this.yDECALAGE = 30*this.facteur;
		} else {
			this.facteur = 5;
			this.xDECALAGE = 25*this.facteur;
			this.yDECALAGE = 50*this.facteur;
		}
	}
	
	public int[] getDecalage() {
		return new int[] {this.facteur, this.xDECALAGE, this.yDECALAGE};
	}
	
	public void paintComponent(Graphics g) {
		this.g = (Graphics2D) g;
		super.paintComponent(g);
		
		int i;
		double[] tmp, tmp2;

		
		if (!xPeriment.isCompleteSimulation()) {
			if (xPeriment.viewVoronoiEnabled()){
				if (this.polygons != null) {
					Point2D[] polygon = null;;
					int j=0;
					for (i = 0;i<polygons.size();i++){
						polygon = polygons.get(i);
						Point2D[] points = new Point2D[polygon.length];
				        for (j = 0; j < polygon.length; j++) {
				        	points[j] = new Point2D.Double(polygon[j].getX()*facteur + xDECALAGE, -polygon[j].getY()*facteur + yDECALAGE);
				        	if ((j>0) && (j <= polygon.length-1)) {
								this.g.setColor(Color.white);
				        		this.g.draw(new Line2D.Double(points[j-1],points[j]));
				        	}
				        	if (j == polygon.length-1){
								this.g.setColor(Color.white);
				        		this.g.draw(new Line2D.Double(points[0],points[j]));
				        	}
				        }
					}
				}
			}
			
			if (xPeriment.isOdometry()) {
				if (position != null) {
						tmp = position.get(position.size()-1);
						this.g.setColor(Color.red);
						this.g.draw(new Rectangle2D.Double(tmp[0]*facteur + xDECALAGE-2.5, -tmp[1]*facteur + yDECALAGE-2.5, 5, 5));
				}
			}
			
			if (xPeriment.isLandmarks()) {
				if ((this.xPeriment.isAntiSlamed()&&(this.scenario==1))){
					this.landmarks = new Vector<double[]>(5,1);
					this.landmarks.add(new double[] {1, 0, 20});
					this.landmarks.add(new double[] {2, 20, 20});
					this.landmarks.add(new double[] {3, 20, 0});
					this.landmarks.add(new double[] {4, 0, 0});
					this.landmarks.add(new double[] {5, 10, 10});
				}
				if (landmarks != null) {
					for(i = 0; i < landmarks.size(); i++) {
						this.g.setColor(Color.black);
						tmp = landmarks.get(i);
						this.g.drawString("["+(int)tmp[0]+"] " + (int)tmp[1] + "," + (int)tmp[2], (int)tmp[1]*facteur + xDECALAGE, -(int)tmp[2]*facteur + yDECALAGE);
						this.g.fillRect((int)tmp[1]*facteur + xDECALAGE, -(int)tmp[2]*facteur + yDECALAGE, 4, 4);
						this.g.setColor(Color.darkGray);
						this.g.setStroke( new BasicStroke(1,BasicStroke.CAP_BUTT,BasicStroke.JOIN_MITER,10.0f,new float[]{5,5},0));
						if (this.xPeriment.viewRangeEnabled()) this.g.drawOval((int)tmp[1]*facteur + xDECALAGE-20*facteur, -(int)tmp[2]*facteur + yDECALAGE-20*facteur, 40*facteur, 40*facteur);
					}
				}
			}

			if (xPeriment.isTrajectory()) {
				if (position != null) {
					for(i = 0; i < position.size()-1; i++) {
						tmp = position.get(i);
						tmp2 = position.get(i+1);
						this.g.setColor(Color.blue);
						this.g.setStroke(new BasicStroke( 1.5f ));
						this.g.draw(new Line2D.Double(tmp[0]*facteur + xDECALAGE, -tmp[1]*facteur + yDECALAGE, tmp2[0]*facteur + xDECALAGE, -tmp2[1]*facteur + yDECALAGE));
					}
				}
			} 
		} else {
			if (xPeriment.viewVoronoiEnabled()){
				if (this.polygons != null) {
					Point2D[] polygon = null;;
					int j=0;
					for (i = 0;i<polygons.size();i++){
						polygon = polygons.get(i);
						Point2D[] points = new Point2D[polygon.length];
				        for (j = 0; j < polygon.length; j++) {
				        	points[j] = new Point2D.Double(polygon[j].getX()*facteur + xDECALAGE, -polygon[j].getY()*facteur + yDECALAGE);
				        	if ((j>0) && (j <= polygon.length-1)) {
								this.g.setColor(Color.white);
				        		this.g.draw(new Line2D.Double(points[j-1],points[j]));
				        	}
				        	if (j == polygon.length-1){
								this.g.setColor(Color.white);
				        		this.g.draw(new Line2D.Double(points[0],points[j]));
				        	}
				        }
					}
				}
			}
			
			if (xPeriment.viewRealEnabled()){
				if (position != null) {
					for(i = 0; i < position.size()-1; i++) {
						tmp = position.get(i);
						tmp2 = position.get(i+1);
						this.g.setColor(Color.blue);
						this.g.setStroke(new BasicStroke( 1.5f ));
						this.g.draw(new Line2D.Double(tmp[0]*facteur + xDECALAGE, -tmp[1]*facteur + yDECALAGE, tmp2[0]*facteur + xDECALAGE, -tmp2[1]*facteur + yDECALAGE));
					}
				}
			}
			
			if (xPeriment.viewErrorEnabled()){
				if (error != null) {
					for(i = 0; i < error.size()-1; i++) {
						tmp = error.get(i);
						tmp2 = error.get(i+1);
						this.g.setColor(Color.green);
						this.g.draw(new Line2D.Double(tmp[0]*facteur + xDECALAGE, -tmp[1]*facteur + yDECALAGE, tmp2[0]*facteur + xDECALAGE, -tmp2[1]*facteur + yDECALAGE));
					}
				}
			}
			
			if (xPeriment.viewFilterEnabled()){
				if (estimate != null) {
					for(i = 0; i < estimate.size()-1; i++) {
						tmp = estimate.get(i);
						tmp2 = estimate.get(i+1);
						this.g.setColor(Color.red);
						this.g.draw(new Line2D.Double(tmp[0]*facteur + xDECALAGE, -tmp[1]*facteur + yDECALAGE, tmp2[0]*facteur + xDECALAGE, -tmp2[1]*facteur + yDECALAGE));
					}
				}
			} 
			
			if (xPeriment.viewAntiEnabled()){
				
			}
			
			if (xPeriment.isLandmarks()) {
				if (landmarks != null) {
					for(i = 0; i < landmarks.size(); i++) {
						this.g.setColor(Color.darkGray);
						tmp = landmarks.get(i);
						this.g.drawString("["+(int)tmp[0]+"] " + (int)tmp[1] + "," + (int)tmp[2], (int)tmp[1]*facteur + xDECALAGE, -(int)tmp[2]*facteur + yDECALAGE);
						this.g.fillRect((int)tmp[1]*facteur + xDECALAGE, -(int)tmp[2]*facteur + yDECALAGE, 4, 4);
						this.g.setColor(Color.darkGray);
						this.g.setStroke( new BasicStroke(1,BasicStroke.CAP_BUTT,BasicStroke.JOIN_MITER,10.0f,new float[]{5,5},0));
						if (this.xPeriment.viewRangeEnabled()) this.g.drawOval((int)tmp[1]*facteur + xDECALAGE-20*facteur, -(int)tmp[2]*facteur + yDECALAGE-20*facteur, 40*facteur, 40*facteur);
					}
				}
			}
			
			if (xPeriment.isOdometry()) {
				if ((position != null) && (position.size() > 1)) {
						tmp = position.get(position.size()-1);
						this.g.setColor(Color.magenta);
						this.g.draw(new Rectangle2D.Double(tmp[0]*facteur + xDECALAGE-2.5, -tmp[1]*facteur + yDECALAGE-2.5, 5, 5));
				}
				if ((estimate != null) && (estimate.size() > 1)) {
					tmp = estimate.get(estimate.size()-1);
					this.g.setColor(Color.magenta);
					this.g.draw(new Rectangle2D.Double(tmp[0]*facteur + xDECALAGE-2.5, -tmp[1]*facteur + yDECALAGE-2.5, 5, 5));
				}
				if ((error != null) && (error.size() > 1)) {
					tmp = error.get(error.size()-1);
					this.g.setColor(Color.magenta);
					this.g.draw(new Rectangle2D.Double(tmp[0]*facteur + xDECALAGE-2.5, -tmp[1]*facteur + yDECALAGE-2.5, 5, 5));
				}
			}
		}

	}
}